#nullable enable

// namespace System.Collections.Generic {
namespace Collections {

// Licensed to the .NET Foundation under one or more agreements.
// The .NET Foundation licenses this file to you under the MIT license.

// ported from:
// https://github.com/dotnet/runtime/blob/main/src/libraries/System.Collections/src/System/Collections/Generic/PriorityQueue.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using System.Diagnostics.CodeAnalysis;
using System.Runtime.CompilerServices;

internal sealed class PriorityQueueDebugView<TElement, TPriority> {
    private readonly PriorityQueue<TElement, TPriority> _queue;
    private readonly bool _sort;

    [DebuggerBrowsable( DebuggerBrowsableState.RootHidden )]
    public (TElement Element, TPriority Priority)[] Items {
        get {
            List<(TElement Element, TPriority Priority)> list = new( _queue.UnorderedItems );
            if ( _sort ) {
                list.Sort( ( i1, i2 ) => _queue.Comparer.Compare( i1.Priority, i2.Priority ) );
            }

            return list.ToArray();
        }
    }

    public PriorityQueueDebugView( PriorityQueue<TElement, TPriority> queue ) {
        ArgumentNullException.ThrowIfNull( queue );

        _queue = queue;
        _sort = true;
    }

    public PriorityQueueDebugView( PriorityQueue<TElement, TPriority>.UnorderedItemsCollection collection ) =>
        _queue = collection?._queue ?? throw new System.ArgumentNullException( nameof(collection) );
}

[SuppressMessage( "ReSharper", "InconsistentNaming" )]
internal static class SR {
    internal const string ArgumentOutOfRange_NeedNonNegNum = "Non-negative number required.";
    internal const string ArgumentOutOfRange_IndexMustBeLessOrEqual = "Index must be less or equal";
    internal const string InvalidOperation_EmptyQueue = "The queue is empty.";
    internal const string InvalidOperation_EnumFailedVersion = "Collection modified while iterating over it.";
    internal const string Arg_NonZeroLowerBound = "Non-zero lower bound required.";
    internal const string Arg_RankMultiDimNotSupported = "Multi-dimensional arrays not supported.";
    internal const string Argument_InvalidArrayType = "Invalid array type.";
    internal const string Argument_InvalidOffLen = "Invalid offset or length.";
}

internal static class ArgumentNullException {
    public static void ThrowIfNull( object o ) {
        if ( o == null ) {
            throw new System.ArgumentNullException(); // hard to do it differently without C# 10's features
        }
    }
}

internal static class ArrayEx {
    internal const int MaxLength = int.MaxValue;
}

/// <summary>
///     Internal helper functions for working with enumerables.
/// </summary>
internal static class EnumerableHelpers {
    /// <summary>Converts an enumerable to an array using the same logic as List{T}.</summary>
    /// <param name="source">The enumerable to convert.</param>
    /// <param name="length">The number of items stored in the resulting array, 0-indexed.</param>
    /// <returns>
    ///     The resulting array.  The length of the array may be greater than <paramref name="length" />,
    ///     which is the actual number of elements in the array.
    /// </returns>
    internal static T[] ToArray<T>( IEnumerable<T> source, out int length ) {
        if ( source is ICollection<T> ic ) {
            int count = ic.Count;
            if ( count != 0 ) {
                // Allocate an array of the desired size, then copy the elements into it. Note that this has the same
                // issue regarding concurrency as other existing collections like List<T>. If the collection size
                // concurrently changes between the array allocation and the CopyTo, we could end up either getting an
                // exception from overrunning the array (if the size went up) or we could end up not filling as many
                // items as 'count' suggests (if the size went down).  This is only an issue for concurrent collections
                // that implement ICollection<T>, which as of .NET 4.6 is just ConcurrentDictionary<TKey, TValue>.
                T[] arr = new T[count];
                ic.CopyTo( arr, 0 );
                length = count;
                return arr;
            }
        } else {
            using ( IEnumerator<T> en = source.GetEnumerator() ) {
                if ( en.MoveNext() ) {
                    const int DefaultCapacity = 4;
                    T[] arr = new T[DefaultCapacity];
                    arr[0] = en.Current;
                    int count = 1;

                    while ( en.MoveNext() ) {
                        if ( count == arr.Length ) {
                            // This is the same growth logic as in List<T>:
                            // If the array is currently empty, we make it a default size.  Otherwise, we attempt to
                            // double the size of the array.  Doubling will overflow once the size of the array reaches
                            // 2^30, since doubling to 2^31 is 1 larger than Int32.MaxValue.  In that case, we instead
                            // constrain the length to be Array.MaxLength (this overflow check works because of the
                            // cast to uint).
                            int newLength = count << 1;
                            if ( (uint) newLength > ArrayEx.MaxLength ) {
                                newLength = ArrayEx.MaxLength <= count ? count + 1 : ArrayEx.MaxLength;
                            }

                            Array.Resize( ref arr, newLength );
                        }

                        arr[count++] = en.Current;
                    }

                    length = count;
                    return arr;
                }
            }
        }

        length = 0;
        return Array.Empty<T>();
    }
}

/// <summary>
///     Represents a min priority queue.
/// </summary>
/// <typeparam name="TElement">Specifies the type of elements in the queue.</typeparam>
/// <typeparam name="TPriority">Specifies the type of priority associated with enqueued elements.</typeparam>
/// <remarks>
///     Implements an array-backed quaternary min-heap. Each element is enqueued with an associated priority
///     that determines the dequeue order: elements with the lowest priority get dequeued first.
/// </remarks>
[DebuggerDisplay( "Count = {Count}" )]
[DebuggerTypeProxy( typeof(PriorityQueueDebugView<,>) )]
public class PriorityQueue<TElement, TPriority> {

    /// <summary>
    ///     Specifies the arity of the d-ary heap, which here is quaternary.
    ///     It is assumed that this value is a power of 2.
    /// </summary>
    private const int Arity = 4;

    /// <summary>
    ///     The binary logarithm of <see cref="Arity" />.
    /// </summary>
    private const int Log2Arity = 2;

    /// <summary>
    ///     Represents an implicit heap-ordered complete d-ary tree, stored as an array.
    /// </summary>
    private (TElement Element, TPriority Priority)[] _nodes;

    /// <summary>
    ///     Custom comparer used to order the heap.
    /// </summary>
    private readonly IComparer<TPriority>? _comparer;

    /// <summary>
    ///     Lazily-initialized collection used to expose the contents of the queue.
    /// </summary>
    private UnorderedItemsCollection? _unorderedItems;

    /// <summary>
    ///     The number of nodes in the heap.
    /// </summary>
    private int _size;

    /// <summary>
    ///     Version updated on mutation to help validate enumerators operate on a consistent state.
    /// </summary>
    private int _version;

    /// <summary>
    ///     Gets the number of elements contained in the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    public int Count => _size;

    /// <summary>
    ///     Gets the priority comparer used by the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    public IComparer<TPriority> Comparer => _comparer ?? Comparer<TPriority>.Default;

    /// <summary>
    ///     Gets a collection that enumerates the elements of the queue in an unordered manner.
    /// </summary>
    /// <remarks>
    ///     The enumeration does not order items by priority, since that would require N * log(N) time and N space.
    ///     Items are instead enumerated following the internal array heap layout.
    /// </remarks>
    public UnorderedItemsCollection UnorderedItems => _unorderedItems ??= new( this );

#if DEBUG
    static PriorityQueue() {
        Debug.Assert( Log2Arity > 0 && Math.Pow( 2, Log2Arity ) == Arity );
    }
#endif

    /// <summary>
    ///     Initializes a new instance of the <see cref="PriorityQueue{TElement, TPriority}" /> class.
    /// </summary>
    public PriorityQueue() {
        _nodes = Array.Empty<(TElement, TPriority)>();
        _comparer = InitializeComparer( null );
    }

    /// <summary>
    ///     Initializes a new instance of the <see cref="PriorityQueue{TElement, TPriority}" /> class
    ///     with the specified initial capacity.
    /// </summary>
    /// <param name="initialCapacity">Initial capacity to allocate in the underlying heap array.</param>
    /// <exception cref="ArgumentOutOfRangeException">
    ///     The specified <paramref name="initialCapacity" /> was negative.
    /// </exception>
    public PriorityQueue( int initialCapacity )
        : this( initialCapacity, comparer: null ) {
    }

    /// <summary>
    ///     Initializes a new instance of the <see cref="PriorityQueue{TElement, TPriority}" /> class
    ///     with the specified custom priority comparer.
    /// </summary>
    /// <param name="comparer">
    ///     Custom comparer dictating the ordering of elements.
    ///     Uses <see cref="Comparer{T}.Default" /> if the argument is <see langword="null" />.
    /// </param>
    public PriorityQueue( IComparer<TPriority>? comparer ) {
        _nodes = Array.Empty<(TElement, TPriority)>();
        _comparer = InitializeComparer( comparer );
    }

    /// <summary>
    ///     Initializes a new instance of the <see cref="PriorityQueue{TElement, TPriority}" /> class
    ///     with the specified initial capacity and custom priority comparer.
    /// </summary>
    /// <param name="initialCapacity">Initial capacity to allocate in the underlying heap array.</param>
    /// <param name="comparer">
    ///     Custom comparer dictating the ordering of elements.
    ///     Uses <see cref="Comparer{T}.Default" /> if the argument is <see langword="null" />.
    /// </param>
    /// <exception cref="ArgumentOutOfRangeException">
    ///     The specified <paramref name="initialCapacity" /> was negative.
    /// </exception>
    public PriorityQueue( int initialCapacity, IComparer<TPriority>? comparer ) {
        if ( initialCapacity < 0 ) {
            throw new ArgumentOutOfRangeException(
                nameof(initialCapacity), initialCapacity, SR.ArgumentOutOfRange_NeedNonNegNum );
        }

        _nodes = new (TElement, TPriority)[initialCapacity];
        _comparer = InitializeComparer( comparer );
    }

    /// <summary>
    ///     Initializes a new instance of the <see cref="PriorityQueue{TElement, TPriority}" /> class
    ///     that is populated with the specified elements and priorities.
    /// </summary>
    /// <param name="items">The pairs of elements and priorities with which to populate the queue.</param>
    /// <exception cref="ArgumentNullException">
    ///     The specified <paramref name="items" /> argument was <see langword="null" />.
    /// </exception>
    /// <remarks>
    ///     Constructs the heap using a heapify operation,
    ///     which is generally faster than enqueuing individual elements sequentially.
    /// </remarks>
    public PriorityQueue( IEnumerable<(TElement Element, TPriority Priority)> items )
        : this( items, comparer: null ) {
    }

    /// <summary>
    ///     Initializes a new instance of the <see cref="PriorityQueue{TElement, TPriority}" /> class
    ///     that is populated with the specified elements and priorities,
    ///     and with the specified custom priority comparer.
    /// </summary>
    /// <param name="items">The pairs of elements and priorities with which to populate the queue.</param>
    /// <param name="comparer">
    ///     Custom comparer dictating the ordering of elements.
    ///     Uses <see cref="Comparer{T}.Default" /> if the argument is <see langword="null" />.
    /// </param>
    /// <exception cref="ArgumentNullException">
    ///     The specified <paramref name="items" /> argument was <see langword="null" />.
    /// </exception>
    /// <remarks>
    ///     Constructs the heap using a heapify operation,
    ///     which is generally faster than enqueuing individual elements sequentially.
    /// </remarks>
    public PriorityQueue( IEnumerable<(TElement Element, TPriority Priority)> items, IComparer<TPriority>? comparer ) {
        ArgumentNullException.ThrowIfNull( items );

        _nodes = EnumerableHelpers.ToArray( items, out _size );
        _comparer = InitializeComparer( comparer );

        if ( _size > 1 ) {
            Heapify();
        }
    }

    /// <summary>
    ///     Adds the specified element with associated priority to the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    /// <param name="element">The element to add to the <see cref="PriorityQueue{TElement, TPriority}" />.</param>
    /// <param name="priority">The priority with which to associate the new element.</param>
    public void Enqueue( TElement element, TPriority priority ) {
        // Virtually add the node at the end of the underlying array.
        // Note that the node being enqueued does not need to be physically placed
        // there at this point, as such an assignment would be redundant.

        int currentSize = _size++;
        _version++;

        if ( _nodes.Length == currentSize ) {
            Grow( currentSize + 1 );
        }

        if ( _comparer == null ) {
            MoveUpDefaultComparer( ( element, priority ), currentSize );
        } else {
            MoveUpCustomComparer( ( element, priority ), currentSize );
        }
    }

    /// <summary>
    ///     Returns the minimal element from the <see cref="PriorityQueue{TElement, TPriority}" /> without removing it.
    /// </summary>
    /// <exception cref="InvalidOperationException">The <see cref="PriorityQueue{TElement, TPriority}" /> is empty.</exception>
    /// <returns>The minimal element of the <see cref="PriorityQueue{TElement, TPriority}" />.</returns>
    public TElement Peek() {
        if ( _size == 0 ) {
            throw new InvalidOperationException( SR.InvalidOperation_EmptyQueue );
        }

        return _nodes[0].Element;
    }

    /// <summary>
    ///     Removes and returns the minimal element from the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    /// <exception cref="InvalidOperationException">The queue is empty.</exception>
    /// <returns>The minimal element of the <see cref="PriorityQueue{TElement, TPriority}" />.</returns>
    public TElement Dequeue() {
        if ( _size == 0 ) {
            throw new InvalidOperationException( SR.InvalidOperation_EmptyQueue );
        }

        TElement element = _nodes[0].Element;
        RemoveRootNode();
        return element;
    }

    /// <summary>
    ///     Removes the minimal element from the <see cref="PriorityQueue{TElement, TPriority}" />,
    ///     and copies it to the <paramref name="element" /> parameter,
    ///     and its associated priority to the <paramref name="priority" /> parameter.
    /// </summary>
    /// <param name="element">The removed element.</param>
    /// <param name="priority">The priority associated with the removed element.</param>
    /// <returns>
    ///     <see langword="true" /> if the element is successfully removed;
    ///     <see langword="false" /> if the <see cref="PriorityQueue{TElement, TPriority}" /> is empty.
    /// </returns>
    public bool TryDequeue( [MaybeNullWhen( false )] out TElement element,
                            [MaybeNullWhen( false )] out TPriority priority ) {
        if ( _size != 0 ) {
            ( element, priority ) = _nodes[0];
            RemoveRootNode();
            return true;
        }

        element = default;
        priority = default;
        return false;
    }

    /// <summary>
    ///     Returns a value that indicates whether there is a minimal element in the
    ///     <see cref="PriorityQueue{TElement, TPriority}" />,
    ///     and if one is present, copies it to the <paramref name="element" /> parameter,
    ///     and its associated priority to the <paramref name="priority" /> parameter.
    ///     The element is not removed from the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    /// <param name="element">The minimal element in the queue.</param>
    /// <param name="priority">The priority associated with the minimal element.</param>
    /// <returns>
    ///     <see langword="true" /> if there is a minimal element;
    ///     <see langword="false" /> if the <see cref="PriorityQueue{TElement, TPriority}" /> is empty.
    /// </returns>
    public bool TryPeek( [MaybeNullWhen( false )] out TElement element,
                         [MaybeNullWhen( false )] out TPriority priority ) {
        if ( _size != 0 ) {
            ( element, priority ) = _nodes[0];
            return true;
        }

        element = default;
        priority = default;
        return false;
    }

    /// <summary>
    ///     Adds the specified element with associated priority to the <see cref="PriorityQueue{TElement, TPriority}" />,
    ///     and immediately removes the minimal element, returning the result.
    /// </summary>
    /// <param name="element">The element to add to the <see cref="PriorityQueue{TElement, TPriority}" />.</param>
    /// <param name="priority">The priority with which to associate the new element.</param>
    /// <returns>The minimal element removed after the enqueue operation.</returns>
    /// <remarks>
    ///     Implements an insert-then-extract heap operation that is generally more efficient
    ///     than sequencing Enqueue and Dequeue operations: in the worst case scenario only one
    ///     shift-down operation is required.
    /// </remarks>
    public TElement EnqueueDequeue( TElement element, TPriority priority ) {
        if ( _size != 0 ) {
            (TElement Element, TPriority Priority) root = _nodes[0];

            if ( _comparer == null ) {
                if ( Comparer<TPriority>.Default.Compare( priority, root.Priority ) > 0 ) {
                    MoveDownDefaultComparer( ( element, priority ), 0 );
                    _version++;
                    return root.Element;
                }
            } else {
                if ( _comparer.Compare( priority, root.Priority ) > 0 ) {
                    MoveDownCustomComparer( ( element, priority ), 0 );
                    _version++;
                    return root.Element;
                }
            }
        }

        return element;
    }

    /// <summary>
    ///     Enqueues a sequence of element/priority pairs to the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    /// <param name="items">The pairs of elements and priorities to add to the queue.</param>
    /// <exception cref="ArgumentNullException">
    ///     The specified <paramref name="items" /> argument was <see langword="null" />.
    /// </exception>
    public void EnqueueRange( IEnumerable<(TElement Element, TPriority Priority)> items ) {
        ArgumentNullException.ThrowIfNull( items );

        int count = 0;
        ICollection<(TElement Element, TPriority Priority)>? collection =
            items as ICollection<(TElement Element, TPriority Priority)>;
        if ( collection is not null && ( count = collection.Count ) > _nodes.Length - _size ) {
            Grow( _size + count );
        }

        if ( _size == 0 ) {
            // build using Heapify() if the queue is empty.

            if ( collection is not null ) {
                collection.CopyTo( _nodes, 0 );
                _size = count;
            } else {
                int i = 0;
                (TElement, TPriority)[] nodes = _nodes;
                foreach ( ( TElement element, TPriority priority ) in items ) {
                    if ( nodes.Length == i ) {
                        Grow( i + 1 );
                        nodes = _nodes;
                    }

                    nodes[i++] = ( element, priority );
                }

                _size = i;
            }

            _version++;

            if ( _size > 1 ) {
                Heapify();
            }
        } else {
            foreach ( ( TElement element, TPriority priority ) in items ) {
                Enqueue( element, priority );
            }
        }
    }

    /// <summary>
    ///     Enqueues a sequence of elements pairs to the <see cref="PriorityQueue{TElement, TPriority}" />,
    ///     all associated with the specified priority.
    /// </summary>
    /// <param name="elements">The elements to add to the queue.</param>
    /// <param name="priority">The priority to associate with the new elements.</param>
    /// <exception cref="ArgumentNullException">
    ///     The specified <paramref name="elements" /> argument was <see langword="null" />.
    /// </exception>
    public void EnqueueRange( IEnumerable<TElement> elements, TPriority priority ) {
        ArgumentNullException.ThrowIfNull( elements );

        int count;
        if ( elements is ICollection<(TElement Element, TPriority Priority)> collection &&
            ( count = collection.Count ) > _nodes.Length - _size ) {
            Grow( _size + count );
        }

        if ( _size == 0 ) {
            // build using Heapify() if the queue is empty.

            int i = 0;
            (TElement, TPriority)[] nodes = _nodes;
            foreach ( TElement element in elements ) {
                if ( nodes.Length == i ) {
                    Grow( i + 1 );
                    nodes = _nodes;
                }

                nodes[i++] = ( element, priority );
            }

            _size = i;
            _version++;

            if ( i > 1 ) {
                Heapify();
            }
        } else {
            foreach ( TElement element in elements ) {
                Enqueue( element, priority );
            }
        }
    }

    /// <summary>
    ///     Removes all items from the <see cref="PriorityQueue{TElement, TPriority}" />.
    /// </summary>
    public void Clear() {
        if ( RuntimeHelpers.IsReferenceOrContainsReferences<(TElement, TPriority)>() ) {
            // Clear the elements so that the gc can reclaim the references
            Array.Clear( _nodes, 0, _size );
        }

        _size = 0;
        _version++;
    }

    /// <summary>
    ///     Ensures that the <see cref="PriorityQueue{TElement, TPriority}" /> can hold up to
    ///     <paramref name="capacity" /> items without further expansion of its backing storage.
    /// </summary>
    /// <param name="capacity">The minimum capacity to be used.</param>
    /// <exception cref="ArgumentOutOfRangeException">
    ///     The specified <paramref name="capacity" /> is negative.
    /// </exception>
    /// <returns>The current capacity of the <see cref="PriorityQueue{TElement, TPriority}" />.</returns>
    public int EnsureCapacity( int capacity ) {
        if ( capacity < 0 ) {
            throw new ArgumentOutOfRangeException( nameof(capacity), capacity, SR.ArgumentOutOfRange_NeedNonNegNum );
        }

        if ( _nodes.Length < capacity ) {
            Grow( capacity );
            _version++;
        }

        return _nodes.Length;
    }

    /// <summary>
    ///     Sets the capacity to the actual number of items in the <see cref="PriorityQueue{TElement, TPriority}" />,
    ///     if that is less than 90 percent of current capacity.
    /// </summary>
    /// <remarks>
    ///     This method can be used to minimize a collection's memory overhead
    ///     if no new elements will be added to the collection.
    /// </remarks>
    public void TrimExcess() {
        int threshold = (int) ( _nodes.Length * 0.9 );
        if ( _size < threshold ) {
            Array.Resize( ref _nodes, _size );
            _version++;
        }
    }

    /// <summary>
    ///     Grows the priority queue to match the specified min capacity.
    /// </summary>
    private void Grow( int minCapacity ) {
        Debug.Assert( _nodes.Length < minCapacity );

        const int GrowFactor = 2;
        const int MinimumGrow = 4;

        int newcapacity = GrowFactor * _nodes.Length;

        // Allow the queue to grow to maximum possible capacity (~2G elements) before encountering overflow.
        // Note that this check works even when _nodes.Length overflowed thanks to the (uint) cast
        if ( (uint) newcapacity > ArrayEx.MaxLength ) {
            newcapacity = ArrayEx.MaxLength;
        }

        // Ensure minimum growth is respected.
        newcapacity = Math.Max( newcapacity, _nodes.Length + MinimumGrow );

        // If the computed capacity is still less than specified, set to the original argument.
        // Capacities exceeding Array.MaxLength will be surfaced as OutOfMemoryException by Array.Resize.
        if ( newcapacity < minCapacity ) {
            newcapacity = minCapacity;
        }

        Array.Resize( ref _nodes, newcapacity );
    }

    /// <summary>
    ///     Removes the node from the root of the heap
    /// </summary>
    private void RemoveRootNode() {
        int lastNodeIndex = --_size;
        _version++;

        if ( lastNodeIndex > 0 ) {
            (TElement Element, TPriority Priority) lastNode = _nodes[lastNodeIndex];
            if ( _comparer == null ) {
                MoveDownDefaultComparer( lastNode, 0 );
            } else {
                MoveDownCustomComparer( lastNode, 0 );
            }
        }

        if ( RuntimeHelpers.IsReferenceOrContainsReferences<(TElement, TPriority)>() ) {
            _nodes[lastNodeIndex] = default;
        }
    }

    /// <summary>
    ///     Gets the index of an element's parent.
    /// </summary>
    private static int GetParentIndex( int index ) => ( index - 1 ) >> Log2Arity;

    /// <summary>
    ///     Gets the index of the first child of an element.
    /// </summary>
    private static int GetFirstChildIndex( int index ) => ( index << Log2Arity ) + 1;

    /// <summary>
    ///     Converts an unordered list into a heap.
    /// </summary>
    private void Heapify() {
        // Leaves of the tree are in fact 1-element heaps, for which there
        // is no need to correct them. The heap property needs to be restored
        // only for higher nodes, starting from the first node that has children.
        // It is the parent of the very last element in the array.

        (TElement Element, TPriority Priority)[] nodes = _nodes;
        int lastParentWithChildren = GetParentIndex( _size - 1 );

        if ( _comparer == null ) {
            for ( int index = lastParentWithChildren; index >= 0; --index ) {
                MoveDownDefaultComparer( nodes[index], index );
            }
        } else {
            for ( int index = lastParentWithChildren; index >= 0; --index ) {
                MoveDownCustomComparer( nodes[index], index );
            }
        }
    }

    /// <summary>
    ///     Moves a node up in the tree to restore heap order.
    /// </summary>
    private void MoveUpDefaultComparer( (TElement Element, TPriority Priority) node, int nodeIndex ) {
        // Instead of swapping items all the way to the root, we will perform
        // a similar optimization as in the insertion sort.

        Debug.Assert( _comparer is null );
        Debug.Assert( 0 <= nodeIndex && nodeIndex < _size );

        (TElement Element, TPriority Priority)[] nodes = _nodes;

        while ( nodeIndex > 0 ) {
            int parentIndex = GetParentIndex( nodeIndex );
            (TElement Element, TPriority Priority) parent = nodes[parentIndex];

            if ( Comparer<TPriority>.Default.Compare( node.Priority, parent.Priority ) < 0 ) {
                nodes[nodeIndex] = parent;
                nodeIndex = parentIndex;
            } else {
                break;
            }
        }

        nodes[nodeIndex] = node;
    }

    /// <summary>
    ///     Moves a node up in the tree to restore heap order.
    /// </summary>
    private void MoveUpCustomComparer( (TElement Element, TPriority Priority) node, int nodeIndex ) {
        // Instead of swapping items all the way to the root, we will perform
        // a similar optimization as in the insertion sort.

        Debug.Assert( _comparer is not null );
        Debug.Assert( 0 <= nodeIndex && nodeIndex < _size );

        IComparer<TPriority> comparer = _comparer;
        (TElement Element, TPriority Priority)[] nodes = _nodes;

        while ( nodeIndex > 0 ) {
            int parentIndex = GetParentIndex( nodeIndex );
            (TElement Element, TPriority Priority) parent = nodes[parentIndex];

            if ( comparer.Compare( node.Priority, parent.Priority ) < 0 ) {
                nodes[nodeIndex] = parent;
                nodeIndex = parentIndex;
            } else {
                break;
            }
        }

        nodes[nodeIndex] = node;
    }

    /// <summary>
    ///     Moves a node down in the tree to restore heap order.
    /// </summary>
    private void MoveDownDefaultComparer( (TElement Element, TPriority Priority) node, int nodeIndex ) {
        // The node to move down will not actually be swapped every time.
        // Rather, values on the affected path will be moved up, thus leaving a free spot
        // for this value to drop in. Similar optimization as in the insertion sort.

        Debug.Assert( _comparer is null );
        Debug.Assert( 0 <= nodeIndex && nodeIndex < _size );

        (TElement Element, TPriority Priority)[] nodes = _nodes;
        int size = _size;

        int i;
        while ( ( i = GetFirstChildIndex( nodeIndex ) ) < size ) {
            // Find the child node with the minimal priority
            (TElement Element, TPriority Priority) minChild = nodes[i];
            int minChildIndex = i;

            int childIndexUpperBound = Math.Min( i + Arity, size );
            while ( ++i < childIndexUpperBound ) {
                (TElement Element, TPriority Priority) nextChild = nodes[i];
                if ( Comparer<TPriority>.Default.Compare( nextChild.Priority, minChild.Priority ) < 0 ) {
                    minChild = nextChild;
                    minChildIndex = i;
                }
            }

            // Heap property is satisfied; insert node in this location.
            if ( Comparer<TPriority>.Default.Compare( node.Priority, minChild.Priority ) <= 0 ) {
                break;
            }

            // Move the minimal child up by one node and
            // continue recursively from its location.
            nodes[nodeIndex] = minChild;
            nodeIndex = minChildIndex;
        }

        nodes[nodeIndex] = node;
    }

    /// <summary>
    ///     Moves a node down in the tree to restore heap order.
    /// </summary>
    private void MoveDownCustomComparer( (TElement Element, TPriority Priority) node, int nodeIndex ) {
        // The node to move down will not actually be swapped every time.
        // Rather, values on the affected path will be moved up, thus leaving a free spot
        // for this value to drop in. Similar optimization as in the insertion sort.

        Debug.Assert( _comparer is not null );
        Debug.Assert( 0 <= nodeIndex && nodeIndex < _size );

        IComparer<TPriority> comparer = _comparer;
        (TElement Element, TPriority Priority)[] nodes = _nodes;
        int size = _size;

        int i;
        while ( ( i = GetFirstChildIndex( nodeIndex ) ) < size ) {
            // Find the child node with the minimal priority
            (TElement Element, TPriority Priority) minChild = nodes[i];
            int minChildIndex = i;

            int childIndexUpperBound = Math.Min( i + Arity, size );
            while ( ++i < childIndexUpperBound ) {
                (TElement Element, TPriority Priority) nextChild = nodes[i];
                if ( comparer.Compare( nextChild.Priority, minChild.Priority ) < 0 ) {
                    minChild = nextChild;
                    minChildIndex = i;
                }
            }

            // Heap property is satisfied; insert node in this location.
            if ( comparer.Compare( node.Priority, minChild.Priority ) <= 0 ) {
                break;
            }

            // Move the minimal child up by one node and continue recursively from its location.
            nodes[nodeIndex] = minChild;
            nodeIndex = minChildIndex;
        }

        nodes[nodeIndex] = node;
    }

    /// <summary>
    ///     Initializes the custom comparer to be used internally by the heap.
    /// </summary>
    private static IComparer<TPriority>? InitializeComparer( IComparer<TPriority>? comparer ) {
        if ( typeof(TPriority).IsValueType ) {
            if ( comparer == Comparer<TPriority>.Default ) {
                // if the user manually specifies the default comparer,
                // revert to using the optimized path.
                return null;
            }

            return comparer;
        } else {
            // Currently the JIT doesn't optimize direct Comparer<T>.Default.Compare
            // calls for reference types, so we want to cache the comparer instance instead.
            // TODO https://github.com/dotnet/runtime/issues/10050: Update if this changes in the future.
            return comparer ?? Comparer<TPriority>.Default;
        }
    }

    /// <summary>
    ///     Enumerates the contents of a <see cref="PriorityQueue{TElement, TPriority}" />, without any ordering guarantees.
    /// </summary>
    [DebuggerDisplay( "Count = {Count}" )]
    [DebuggerTypeProxy( typeof(PriorityQueueDebugView<,>) )]
    public sealed class UnorderedItemsCollection : IReadOnlyCollection<(TElement Element, TPriority Priority)>,
                                                   ICollection {
        internal readonly PriorityQueue<TElement, TPriority> _queue;

        internal UnorderedItemsCollection( PriorityQueue<TElement, TPriority> queue ) => _queue = queue;

        /// <summary>
        ///     Returns an enumerator that iterates through the <see cref="UnorderedItems" />.
        /// </summary>
        /// <returns>An <see cref="Enumerator" /> for the <see cref="UnorderedItems" />.</returns>
        public Enumerator GetEnumerator() => new( _queue );

        object ICollection.SyncRoot => this;
        bool ICollection.IsSynchronized => false;

        void ICollection.CopyTo( Array array, int index ) {
            ArgumentNullException.ThrowIfNull( array );

            if ( array.Rank != 1 ) {
                throw new ArgumentException( SR.Arg_RankMultiDimNotSupported, nameof(array) );
            }

            if ( array.GetLowerBound( 0 ) != 0 ) {
                throw new ArgumentException( SR.Arg_NonZeroLowerBound, nameof(array) );
            }

            if ( index < 0 || index > array.Length ) {
                throw new ArgumentOutOfRangeException( nameof(index), index,
                                                       SR.ArgumentOutOfRange_IndexMustBeLessOrEqual );
            }

            if ( array.Length - index < _queue._size ) {
                throw new ArgumentException( SR.Argument_InvalidOffLen );
            }

            try {
                Array.Copy( _queue._nodes, 0, array, index, _queue._size );
            } catch ( ArrayTypeMismatchException ) {
                throw new ArgumentException( SR.Argument_InvalidArrayType, nameof(array) );
            }
        }

        public int Count => _queue._size;

        IEnumerator<(TElement Element, TPriority Priority)> IEnumerable<(TElement Element, TPriority Priority)>.
            GetEnumerator() => GetEnumerator();

        IEnumerator IEnumerable.GetEnumerator() => GetEnumerator();

        /// <summary>
        ///     Enumerates the element and priority pairs of a <see cref="PriorityQueue{TElement, TPriority}" />,
        ///     without any ordering guarantees.
        /// </summary>
        public struct Enumerator : IEnumerator<(TElement Element, TPriority Priority)> {
            private readonly PriorityQueue<TElement, TPriority> _queue;
            private readonly int _version;
            private int _index;

            internal Enumerator( PriorityQueue<TElement, TPriority> queue ) {
                _queue = queue;
                _index = 0;
                _version = queue._version;
                Current = default;
            }

            /// <summary>
            ///     Releases all resources used by the <see cref="Enumerator" />.
            /// </summary>
            public void Dispose() {
            }

            /// <summary>
            ///     Advances the enumerator to the next element of the <see cref="UnorderedItems" />.
            /// </summary>
            /// <returns>
            ///     <see langword="true" /> if the enumerator was successfully advanced to the next element;
            ///     <see langword="false" /> if the enumerator has passed the end of the collection.
            /// </returns>
            public bool MoveNext() {
                PriorityQueue<TElement, TPriority> localQueue = _queue;

                if ( _version == localQueue._version && ( (uint) _index < (uint) localQueue._size ) ) {
                    Current = localQueue._nodes[_index];
                    _index++;
                    return true;
                }

                return MoveNextRare();
            }

            private bool MoveNextRare() {
                if ( _version != _queue._version ) {
                    throw new InvalidOperationException( SR.InvalidOperation_EnumFailedVersion );
                }

                _index = _queue._size + 1;
                Current = default;
                return false;
            }

            /// <summary>
            ///     Gets the element at the current position of the enumerator.
            /// </summary>
            public (TElement Element, TPriority Priority) Current { get; private set; }

            object IEnumerator.Current => Current;

            void IEnumerator.Reset() {
                if ( _version != _queue._version ) {
                    throw new InvalidOperationException( SR.InvalidOperation_EnumFailedVersion );
                }

                _index = 0;
                Current = default;
            }
        }
    }
}

}
